#include "current_sense.h"

volatile PhaseCurrent_t phaseCurrent;

float gainA;
float gainB;
float gainC;

float voltageToAmpRatio;
float shuntResistor = 0.01;
float ampGain = 50;

float zeroOffsetA;
float zeroOffsetB;
float zeroOffsetC;

extern volatile ControlMode_t CURR_MODE;

#define VREF_VOLTAGE 3.31f

void ADC1_2_IRQHandler(void)
{
  if(adc_flag_get(ADC1, ADC_PCCE_FLAG) != RESET)
  {
    adc_flag_clear(ADC1, ADC_PCCE_FLAG);
    
    phaseCurrent.a = -((adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_1) / 4095.0 * VREF_VOLTAGE) - zeroOffsetA) * gainA; // Amps
    phaseCurrent.b = -((adc_preempt_conversion_data_get(ADC2, ADC_PREEMPT_CHANNEL_1) / 4095.0 * VREF_VOLTAGE) - zeroOffsetB) * gainB; // Amps
    
//  if(CURR_MODE != NOT_STARTED)
//  {
//    printf("%.2f,%.2f,%.2f\r\n", current.a, current.b, -(current.a + current.b));
//  }
  }
}

void LowSideCurrentSense_InitAdc(void)
{
  adc_base_config_type adc_base_struct = {0};
  gpio_init_type gpio_initstructure = {0};
  crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE);

  gpio_default_para_init(&gpio_initstructure);
  gpio_initstructure.gpio_mode = GPIO_MODE_ANALOG;
  gpio_initstructure.gpio_pins = GPIO_PINS_6 | GPIO_PINS_7;
  gpio_init(GPIOA, &gpio_initstructure);

  crm_periph_clock_enable(CRM_ADC1_PERIPH_CLOCK, TRUE);
  crm_periph_clock_enable(CRM_ADC2_PERIPH_CLOCK, TRUE);
  crm_adc_clock_div_set(CRM_ADC_DIV_4);
  nvic_irq_enable(ADC1_2_IRQn, 1, 0);

  adc_combine_mode_select(ADC_PREEMPT_SMLT_ONLY_MODE);

  adc_base_default_para_init(&adc_base_struct);
  adc_base_struct.sequence_mode = TRUE;
  adc_base_struct.repeat_mode = FALSE;
  adc_base_struct.data_align = ADC_RIGHT_ALIGNMENT;
  adc_base_struct.ordinary_channel_length = 0;

  adc_base_config(ADC1, &adc_base_struct);
  adc_preempt_channel_length_set(ADC1, 1);
  adc_preempt_channel_set(ADC1, ADC_CHANNEL_6, 1, ADC_SAMPLETIME_7_5);
  adc_preempt_conversion_trigger_set(ADC1, ADC12_PREEMPT_TRIG_TMR1CH4, TRUE);

  adc_base_config(ADC2, &adc_base_struct);
  adc_preempt_channel_length_set(ADC2, 1);
  adc_preempt_channel_set(ADC2, ADC_CHANNEL_7, 1, ADC_SAMPLETIME_7_5);
  adc_preempt_conversion_trigger_set(ADC2, ADC12_PREEMPT_TRIG_TMR1CH4, TRUE);

  adc_enable(ADC1, TRUE);
  adc_enable(ADC2, TRUE);
  adc_calibration_init(ADC1);
  while(adc_calibration_init_status_get(ADC1));
  adc_calibration_start(ADC1);
  while(adc_calibration_status_get(ADC1));
  adc_calibration_init(ADC2);
  while(adc_calibration_init_status_get(ADC2));
  adc_calibration_start(ADC2);
  while(adc_calibration_status_get(ADC2));
}

void LowSideCurrentSense_Init(void)
{
  voltageToAmpRatio = 1.0f / shuntResistor / ampGain; // volts to amps

  gainA = voltageToAmpRatio;
  gainB = voltageToAmpRatio;
  gainC = voltageToAmpRatio;

  LowSideCurrentSense_InitAdc();
  LowSideCurrentSense_CalibrateOffsets();
}

void LowSideCurrentSense_CalibrateOffsets(void)
{
  const int calibrationRounds = 1000;

  zeroOffsetA = 0;
  zeroOffsetB = 0;
  zeroOffsetC = 0;

  adc_flag_clear(ADC1, ADC_PCCE_FLAG);
  for(int i = 0; i < calibrationRounds; i++)
  {
    while(adc_flag_get(ADC1, ADC_PCCE_FLAG) == RESET);
    adc_flag_clear(ADC1, ADC_PCCE_FLAG);
    zeroOffsetA += adc_preempt_conversion_data_get(ADC1, ADC_PREEMPT_CHANNEL_1) / 4095.0 * VREF_VOLTAGE;
    zeroOffsetB += adc_preempt_conversion_data_get(ADC2, ADC_PREEMPT_CHANNEL_1) / 4095.0 * VREF_VOLTAGE;
  }
  zeroOffsetA = zeroOffsetA / calibrationRounds;
  zeroOffsetB = zeroOffsetB / calibrationRounds;
  debug_printf("zeroOffsetA = %.2f, zeroOffsetB=%.2f\r\n", zeroOffsetA, zeroOffsetB);

  adc_interrupt_enable(ADC1, ADC_PCCE_INT, TRUE);
}
DqCurrent_t getFOCCurrents(float angle_el)
{
  // calculate clarke transform
  float i_alpha, i_beta;
  // if only two measured currents
  i_alpha = phaseCurrent.a;
  i_beta = _1_SQRT3 * phaseCurrent.a + _2_SQRT3 * phaseCurrent.b;

  // calculate park transform
  float ct;
  float st;
  st = arm_sin_f32(angle_el);
  ct = arm_cos_f32(angle_el);
  DqCurrent_t return_current;
  return_current.d = i_alpha * ct + i_beta * st;
  return_current.q = i_beta * ct - i_alpha * st;
  return return_current;
}
